#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>

#include "../drv/drv_timer.h"
#include "../drv/drv_leds.h"
#include "../drv/drv_uart.h"
#include "../drv/drv_i2c.h"

#include "bmp085.h"
#include "hmc5883l.h"
#include "l3g4200.h"
#include "venus638.h"
#include "stlis331dlh.h"

#define DEBUG_SENSORS 1

S_Acc acc;
S_Mag mag;
S_Gyro gyro;	
S_Baro baro;
S_Gpgga gps_gga;
//file descriptor i2c
int fd_i2c = -1;
//file descriptor uart
int fd_uart = -1;

//Init sensors
int InitSensors ( void )
{
	//led rouge ON
	//LedOn(LED_RED);

	//init des timers
	//drv_timerInit();
	fd_i2c = drv_i2cOpen();
	
	//Init composant UART
	//fd_uart = VENUS638Init( );

	//Init composants I2c
	if(DEBUG_SENSORS == 1)
	{
		//Init
		L3G4200DInit(fd_i2c);
		STLIS331DLHInit(fd_i2c); 
		//BMP085Init(fd_i2c);
		//HMC5883LInit(fd_i2c);

		//Calibration
		L3G4200DCalibrate(fd_i2c);
		STLIS331DLHCalibrate(fd_i2c);
		//BMP085Calibrate(fd_i2c);
		//HMC5883LCalibrate(fd_i2c);
	}
	//led rouge OFF
	//LedOff(LED_RED);
	return 1; 
}

void DeInitSensors ( void )
{
	//stop la liaison I2C
	drv_i2cClose(fd_i2c);
	//stop la liaison UART
	//drv_uartClose(fd_uart);
}

//Lecture sensors
int SensorsRead( void )
{
	L3G4200DRead( fd_i2c , &gyro );
	//printf("gyro->x =>\t %0.3f\t\tgyro->y =>\t %0.3f\t\tgyro->z =>\t %0.3f\n", gyro.x, gyro.y, gyro.z);

	STLIS331DLHRead( fd_i2c , &acc );
	//printf("acc->x =>\t %0.3f\t\tacc->y =>\t %0.3f\t\tacc->z =>\t %0.3f\n", acc.x, acc.y, acc.z);

	//HMC5883LRead( fd_i2c , &mag );
	//printf("mag->x =>\t %0.3f\tmag->y =>\t %0.3f\t\tmag->z =>\t %0.3f\n", mag->x, mag->y, mag->z);

	//BMP085Read( fd_i2c , &baro );
	//printf("temperature =>\t %d\tpressure =>\t %ld\t\taltitude =>\t %f\n", baro->temperature, baro->pressure, baro->altitude);

	//VENUS638Read(&gps_gga);	
	//printf("altitude =>\t %s\tlatitude =>\t %s\tlongitude =>\t %s\n", gps_gga->altitude, gps_gga->latitude, gps_gga->longitude);
	

	return 1;
}

static float gyroXangle=0;
static float gyroYangle=0;
static float gyroZangle=0;
int SensorsCompute( double delta_time )
{
	//printf ("7 ^ 3 = %f\n", pow (7.0, 3.0) );
	#define ToDeg(x) (x*(float)(180/M_PI))
	double accXangle;
	double accYangle;
	double accZangle;

	accXangle = (float)atan2((double)(acc.x) , (double)sqrt((double)(pow((double)acc.y,(double)2.0)+pow((double)acc.z,(double)2.0))));
        accYangle = (float)atan2((double)(acc.y) , (double)sqrt((double)(pow((double)acc.x,(double)2.0)+pow((double)acc.z,(double)2.0))));
        
        accXangle = ToDeg(accXangle);
        accYangle = ToDeg(accYangle);
	accZangle = acc.z;
	//printf("accXangle =>\t %0.3f\taccYangle =>\t %0.3f\t\taccZangle =>\t %0.3f\n", accXangle, accYangle, accZangle);

        gyroXangle      +=      (float)((float)((gyro.x * delta_time) / 10000000000.0));  
        gyroYangle      +=      (float)((float)((gyro.y * delta_time) / 10000000000.0));
        gyroZangle      +=      (float)((float)((gyro.z * delta_time) / 10000000000.0));
	printf("gyro->x =>\t %0.3f\tgyro->y =>\t %0.3f\tgyro->z =>\t %0.3f\n", gyroXangle, gyroYangle, gyroZangle);

	return 1; 
}

                


